/**
  ******************************************************************************
  * File Name          : rocr6_driver.cpp
  * Description       : rocr6_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <rocr6_driver/rocr6_driver.h>

#define RT_PI	3.1415926535897931

static std::string model;
static int offset = 0;
static int dir = 0;     //一关节运动方向筛选  0：选择所有解  1：筛选正向解  -1：筛选负向解
//static int mode = 0;
std::string _port;
static int gn = 1;      //逆解组号
static int status = 0;

double rx_data[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double tx_data[JOINT_MAX] = {-1, -1, -1, -1, -1, -1};

double last[JOINT_MAX] = {0, 0, 0, 0, 0, 0};
double last_gripper = 0;

double xlast[JOINT_MAX] = {0, 0, 0, 0, 0, 0};

double dh[] =
{
	0.144,		0,			0,			0.106,			0.114,			0.067,
	0,			0,			-0.264,		-0.236,			0,				0,
	0,			RT_PI / 2,	0,			0,				RT_PI / 2,		-RT_PI / 2,
	0,			-RT_PI / 2,	0,			-RT_PI / 2,		0,				0
};

double dh_v2[] =
{
	0.1215,		0,			0,			0.1105,			0.090,			0.082,
	0,			0,			-0.300,		-0.276,			0,				0,
	0,			RT_PI / 2,	0,			0,				RT_PI / 2,		-RT_PI / 2,
	0,			-RT_PI / 2,	0,			-RT_PI / 2,		0,				0
};

double *dh_ptr;

double itheta[48];
double ax[6];
double sp[16];

/* Private function prototypes -----------------------------------------------*/

/**
  * @brief  接收rocr6_msgs/Goal话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void goalCB(const rocr6_msgs::Goal::ConstPtr & msg)
{
  double val,sel;
  int rst = 0;
  if(msg->cmd)//机械臂位姿控制
  { 
    ax[0] = msg->a1;
    ax[1] = msg->a2;
    ax[2] = msg->a3;
    ax[3] = msg->a4;
    ax[4] = msg->a5;
    ax[5] = msg->a6;

    rst = ik6s(ax, dh_ptr, itheta);

    if(rst)
    {
      return;
    }
    else
    {
      priority_path(dir,itheta,xlast, &val, &sel);
      if((val==0)&&(sel==0))
      {
        printf("没有满足条件的最优解. \r\n");
        return;
      }
      if(sel==0)
      {
        gn = 0;
      }
      else
      {
        gn = sel-1;
      }
      printf("运动方向：[%d] 最小路径权重：[%f]->选择第[%d]组解\r\n",dir, val,(int)sel);
      xlast[0] = itheta[gn+0];
      xlast[1] = itheta[gn+8];
      xlast[2] = itheta[gn+16];
      xlast[3] = itheta[gn+24];
      xlast[4] = itheta[gn+32];
      xlast[5] = itheta[gn+40];

/*
      printf("%f %f %f %f %f %f\r\n", 
            xlast[0]*180/RT_PI, 
            xlast[1]*180/RT_PI, 
            xlast[2]*180/RT_PI, 
            xlast[3]*180/RT_PI,
            xlast[4]*180/RT_PI, 
            xlast[5]*180/RT_PI
      );
*/

      last[0] = rocr6_tf_tdata(xlast[0]);
      last[1] = rocr6_tf_tdata(xlast[1]);
      last[2] = rocr6_tf_tdata(xlast[2]);
      last[3] = rocr6_tf_tdata(xlast[3]);
      last[4] = rocr6_tf_tdata(xlast[4]);
      last[5] = rocr6_tf_tdata(xlast[5]);

    }
  }
  else//机械臂角度控制
  {
    last[0] = rocr6_tf_tdata(msg->a1);
    last[1] = rocr6_tf_tdata(msg->a2);
    last[2] = rocr6_tf_tdata(msg->a3);
    last[3] = rocr6_tf_tdata(msg->a4);
    last[4] = rocr6_tf_tdata(msg->a5);
    last[5] = rocr6_tf_tdata(msg->a6);
  }

  if(
    (tx_data[0]==last[0])&&
    (tx_data[1]==last[1])&&
    (tx_data[2]==last[2])&&
    (tx_data[3]==last[3])&&
    (tx_data[4]==last[4])&&
    (tx_data[5]==last[5])
  )
  {
    //发送的角度相同，不用处理
  }
  else
  {
    tx_data[0] = last[0];
    tx_data[1] = last[1];
    tx_data[2] = last[2];
    tx_data[3] = last[3];
    tx_data[4] = last[4];
    tx_data[5] = last[5];

    //控制器插补模式运行
    printf("关节角度:[% 0.4f % 0.4f % 0.4f % 0.4f % 0.4f %0.4f]\r\n", 
        tx_data[0]/10, tx_data[1]/10, tx_data[2]/10, tx_data[3]/10, tx_data[4]/10, tx_data[5]/10);
    rocr6_txSig(1, 0, tx_data);  
  }
}

/**
  * @brief  接收rocr6_msgs/Gripper话题数据
  * @param  消息结构体指针
  * @retval 无
  */
void gripperCB(const rocr6_msgs::Gripper::ConstPtr & msg)
{
  double gripper = msg->state;
  double txd[6]= {0, 0, 0, 0, 0, 0};
  if(gripper == last_gripper)
  {
    //夹爪状态相同，不用处理
  }
      last_gripper = gripper;
      txd[0] = gripper;
      rocr6_txSig(0x55, 0, txd); 
}

/**
  * @brief  机械臂驱动主函数
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
  ros::init(argc, argv, "rocr6_driver");
  ros::NodeHandle nh;
  ros::Rate loop_rate(100);   //100hz
  rocr6_msgs::Feedback joint_fb;

  double dt[6];

  //返回关节实际角度
  ros::Publisher pub = nh.advertise<rocr6_msgs::Feedback>("rocr6_msgs/feedback", 100);

  //订阅角度话题
  ros::Subscriber sub_goal = nh.subscribe("rocr6_msgs/goal", 100, goalCB);
  
  //订阅夹爪话题
  ros::Subscriber sub_gripper = nh.subscribe("rocr6_msgs/gripper", 100, gripperCB);

  nh.getParam("/rocr6_driver/model", model);  
  nh.getParam("/rocr6_driver/joint_dir", dir);
  nh.getParam("/rocr6_driver/joint_offset", offset);
  nh.getParam("/rocr6_driver/port", _port);

  if(model=="v1")
  {
    printf("一代机械臂\r\n");
    dh_ptr = dh;
  }
  else if(model=="v2")
  {
    printf("二代机械臂\r\n");
    dh_ptr = dh_v2;
  }
  else
  {
    printf("请使用有效的机械臂\r\n");
    exit(1);
  }

  if(rocr6_init(_port.c_str()))
  {
    //开启机械臂抱闸，并延时以等待抱闸完全开启
    rocr6_brake(1);
    usleep(6000000);
    printf("开启机械臂数据广播\r\n");
    rocr6_broadcast(1);
  }
  else 
  {
    exit(1);
  }

  // printf("t:X=%f  Y=%f  Z=%f  r=%f  p=%f  y%f\r\n", 
  //     -0.308, 
  //     -0.367, 
  //     0.524, 
  //     -43.898*RT_PI/180, 
  //     25.659*RT_PI/180, 
  //     56.31*RT_PI/180);

  while (ros::ok())
  {
    //关节弧度反馈
    status = rocr6_rxSig(rx_data);
    joint_fb.header.seq+=1;
    joint_fb.header.stamp = ros::Time::now();
	  joint_fb.header.frame_id = "rocr6";

    dt[0] = rocr6_tf_rdata(rx_data[0]); 
    dt[1] = rocr6_tf_rdata(rx_data[1]); 
    dt[2] = rocr6_tf_rdata(rx_data[2]); 
    dt[3] = rocr6_tf_rdata(rx_data[3]); 
    dt[4] = rocr6_tf_rdata(rx_data[4]); 
    dt[5] = rocr6_tf_rdata(rx_data[5]); 
    
    joint_fb.shoulder_pan_joint.position =  dt[0];
    joint_fb.shoulder_lift_joint.position = dt[1];
    joint_fb.elbow_joint.position = dt[2];
    joint_fb.wrist_1_joint.position = dt[3];
    joint_fb.wrist_2_joint.position = dt[4]; 
    joint_fb.wrist_3_joint.position = dt[5];

    // printf("\r\n--------\r\n");
	  fk6s(dt, dh_ptr, sp);
	  // printf("Nx=% 3.4f      Ox=% 3.4f      Ax=% 3.4f      Px=% 3.4f\r\n", sp[0], sp[4], sp[8], sp[12]);
    // printf("Ny=% 3.4f      Oy=% 3.4f      Ay=% 3.4f      Py=% 3.4f\r\n", sp[1], sp[5], sp[9], sp[13]);
    // printf("Nz=% 3.4f      Oz=% 3.4f      Az=% 3.4f      Pz=% 3.4f\r\n", sp[2], sp[6], sp[10], sp[14]);
    // printf("Fn=% 3.4f      Fo=% 3.4f      Fa=% 3.4f      Fz=% 3.4f\r\n", sp[3], sp[7], sp[11], sp[15]);
	  // printf("--------\r\n");

    joint_fb.homogeneous.X[0] = sp[0];
    joint_fb.homogeneous.X[1] = sp[4];
    joint_fb.homogeneous.X[2] = sp[8];
    joint_fb.homogeneous.X[3] = sp[12];

    joint_fb.homogeneous.Y[0] = sp[1];
    joint_fb.homogeneous.Y[1] = sp[5];
    joint_fb.homogeneous.Y[2] = sp[9];
    joint_fb.homogeneous.Y[3] = sp[13];

    joint_fb.homogeneous.Z[0] = sp[2];
    joint_fb.homogeneous.Z[1] = sp[6];
    joint_fb.homogeneous.Z[2] = sp[10];
    joint_fb.homogeneous.Z[3] = sp[14];

    joint_fb.homogeneous.q[0] = sp[3];
    joint_fb.homogeneous.q[1] = sp[7];
    joint_fb.homogeneous.q[2] = sp[11];
    joint_fb.homogeneous.q[3] = sp[15];

    joint_fb.status = status;

    pub.publish(joint_fb);  

    ros::spinOnce();
    loop_rate.sleep();
  }
 
  printf("关闭机械臂数据广播\r\n");
  rocr6_broadcast(0);
  //关闭机械臂抱闸
  //rocr6_brake(0);
  rocr6_deinit();

  return 0;
}
